using System;
using System.Collections.Generic;
using System.Drawing;
using System.Text;

namespace PBR
{
    /// <summary>
    /// Class that interpolates a data set given a KDTree
    /// </summary>
    public class RadialBasisInterp
    {
        #region Static Variables

        private static int NUMNEIGHBORS = 4;
        private static double POWER = 3.0;
        
        #endregion

        #region Member Variables

        private double power = POWER;

        #endregion

        #region Member Functions

        /// <summary>
        /// Constructor
        /// </summary>
        public RadialBasisInterp()
        {
        }

        /// <summary>
        /// Interpolates the data using RBF to determine the value at the point
        /// </summary>
        /// <param name="vectorsKDTree">KDTree with current vectors</param>
        /// <param name="point">The point to look up</param>
        /// <param name="angle"></param>
        /// <param name="brushIndex"></param>
        public void InterpAngle(KDTreeDLL.KDTree vectorsKDTree, Point point, out byte angle, out int brushIndex)
        {
            int numNeighbors = NUMNEIGHBORS;
            if (vectorsKDTree.Count == 0)
            {
                angle = 0;
                brushIndex = 0;
                return;
            }
            else if (vectorsKDTree.Count < NUMNEIGHBORS)
            {
                numNeighbors = vectorsKDTree.Count;
            }

            brushIndex = 0;

            double[] dLoc = new double[2] { point.X, point.Y };

            Object[] nodeList = vectorsKDTree.Nearest(dLoc, numNeighbors);

            float angleF;
            float denominator = 0.0f;
            float numerator = 0.0f;
            float weight;
            float value = 0.0f;

            float angleUp = 0.0f;
            float angleDown = 0.0f;

            // Calculate angle
            foreach(KDNodeData node in nodeList)
            {
                weight = (float)(1.0f / (Math.Pow(Distance(node.point, point.X, point.Y), power)));
                angleF = (float)node.angle;
                if (angleF - value > 127)
                {
                    angleF -= 255;
                }
                else if (angleF - value < -127)
                {
                    angleF += 255;
                }
                numerator += (float)(weight * angleF);
                denominator += weight;
                value = numerator / denominator;
                if (value > 255)
                    { value = (value % 256); }
            }
            angle = (byte)(numerator / denominator);
            if (angle > 255)
            {
                angle = (byte)(angle % 256);
            }

            // Calculate angle up : 90 degree to actual angle
            double cosAngle = Math.Cos(angle/255 * Math.PI);
            double sinAngle = Math.Sign(angle / 255 * Math.PI);

            int xIndex = point.X + (int)Math.Round(cosAngle);
            int yIndex = point.Y + (int)Math.Round(sinAngle);

            denominator = 0.0f;
            numerator = 0.0f;

            foreach (KDNodeData node in nodeList)
            {
                weight = (float)(1.0f / (Math.Pow(Distance(node.point, xIndex, yIndex), power)));
                angleF = (float)node.angle;
                if (angleF - value > 127)
                {
                    angleF -= 255;
                }
                else if (angleF - value < -127)
                {
                    angleF += 255;
                }
                numerator += (float)(weight * angleF);
                denominator += weight;
                value = numerator / denominator;
                if (value > 255)
                { value = (value % 256); }
            }
            angleUp = (byte)(numerator / denominator);
            if (angleUp > 255)
            {
                angleUp = (byte)(angleUp % 256);
            }

            // Calculate angle down : -90 degree to actual angle
            cosAngle = Math.Cos(angle / 255 * Math.PI + Math.PI);
            sinAngle = Math.Sign(angle / 255 * Math.PI + Math.PI);

            xIndex = point.X + (int)Math.Round(cosAngle);
            yIndex = point.Y + (int)Math.Round(sinAngle);

            denominator = 0.0f;
            numerator = 0.0f;

            foreach (KDNodeData node in nodeList)
            {
                weight = (float)(1.0f / (Math.Pow(Distance(node.point, xIndex, yIndex), power)));
                angleF = (float)node.angle;
                if (angleF - value > 127)
                {
                    angleF -= 255;
                }
                else if (angleF - value < -127)
                {
                    angleF += 255;
                }
                numerator += (float)(weight * angleF);
                denominator += weight;
                value = numerator / denominator;
                if (value > 255)
                { value = (value % 256); }
            }
            angleDown = (byte)(numerator / denominator);
            if (angleDown > 255)
            {
                angleDown = (byte)(angleDown % 256);
            }

            double secondDeriv = ((angleUp - angle) + (angle - angleDown)) / 2.0;
            if (secondDeriv < 0)
                secondDeriv += 255;

            if(secondDeriv < 5 || secondDeriv > 250)
            {
                brushIndex = 0;
            }
            else if(secondDeriv < 10)
            {
                brushIndex = 1;
            }
            else if (secondDeriv < 15)
            {
                brushIndex = 2;
            }
            else if (secondDeriv < 20)
            {
                brushIndex = 3;
            }
            else if (secondDeriv > 245)
            {
                brushIndex = 4;
            }
            else if (secondDeriv > 240)
            {
                brushIndex = 5;
            }
            else if (secondDeriv > 235)
            {
                brushIndex = 6;
            }
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="vectorsKDTree"></param>
        /// <param name="point"></param>
        /// <returns></returns>
        public byte InterpAngle(KDTreeDLL.KDTree vectorsKDTree, Point point)
        {
            int bogus;
            byte retVal;
            InterpAngle(vectorsKDTree, point, out retVal, out bogus);
            return retVal;
        }

        //public int InterpBrush(
        
        private double Distance(Point point, int x, int y)
        {
            return Math.Sqrt((point.X - x) * (point.X - x) + (point.Y - y) * (point.Y - y));
        }




        #endregion

    
    }
}
